/**
 * Implement the primitive for geometric computation.
 * @author Songgang Xu (sxu at tamu dot edu)
 * @file primitive.h
 */

#ifndef _XMESH_PRIMITIVE_H
#define _XMESH_PRIMITIVE_H

#include <vector>

extern const double epsilon ;
extern const double epsilon2 ;

namespace math 
{

template <typename Value, unsigned dim> class  vector   ;
template <typename Value, unsigned dim > class Line     ;
template <typename Value, unsigned dim > class Segment  ;
template <typename Value, unsigned dim > class Ray      ;
template <typename Value, unsigned dim > class Plane    ;
template <typename Value, unsigned dim > class Polygon  ;

/**
 * The base class for all the primitives. 
 */
template <typename Value, unsigned dim >
struct Object
{
protected:
	Object() {}
};

/**
 * Line class
 */
template <typename value_, unsigned dim >
	class Line : public Object <value_, dim>
{
protected:
	typedef value_ value_type ; 
	typedef typename vector <value_type, dim>::vector_type vector_type ;
	typedef vector_type point_type; 
public:
	/**
	 * Default Constructor.
	 */
	Line () {}

	/**
	 * Copy Constructor.
	 * @param _l Line object. 
	 */
	Line (const Line & _l)  
		: m_point (_l.m_point), m_vector (_l.m_vector)
	{
	}
	
	/**
	 * Constructor. A line is defined by a point and a vector. 
	 * @param _p Point.
	 * @param _v Vector. 
	 */
	Line (const point_type & _p, const vector_type & _v) 
		: m_point (_p), m_vector (_v){}

public:
	/**
	 * Return const reference one point on the line. 
	 * @return Point. 
	 */
	const point_type & get_point   () const {return this->m_point; }

	/**
	 * Return reference one point on the line. 
	 * @return Point. 
	 */
	point_type & get_point   () {return this->m_point; }

	/**
	 * Return the direction of the line. 
	 * @return Vector of the direction. 
	 */ 
	const vector_type & get_direction () const {return this->m_vector ;}

	/**
	 * Return the direction of the line. 
	 * @return Vector of the direction. 
	 */ 
	vector_type & get_direction () {return this->m_vector ;}

	point_type nearest_point (const point_type & _p)
	{
		point_type p2 = evaluate (value_type(1));
		return evaluate ((p2 - this->m_point ) * (_p - this->m_point));
	}

	/**
	 * Return the nearest distant from the point to the line. 
	 */
	value_type dist_from_point (const point_type & _p) 
	{
		return distance(_p , nearest_point(_p));
	}

	/**
	 * Evaluate the point with paramater s.
	 * p + s * d. p is m_point, d is m_vector. 
	 */
	point_type evaluate (const value_type & s)
	{
		return m_point + m_vector * s ;
	}

	/**
	 * Overload the operator << to output the line. 
	 * @param ostr Output stream. 
	 * @param _l const line object. 
	 */
	friend std::ostream & operator << (std::ostream & ostr, const Line & _l)
	{
		std::cout<<"Point "<<_l.m_point << " Direction " <<_l.m_vector ;
		return ostr ;
	}
	/*
	friend bool operator == (const Line & _lhs, const Line & _rhs)
	{
		if ( !(_lhs.m_vector == _rhs.m_vector)) return false ;
		vector_type v1 = _lhs.m_point - _rhs.m_point ;
		vector_type v2 = _rhs.m_point - _lhs.m_point ;


	}
	*/
private :
	point_type  m_point ;
	vector_type m_vector ;
} ;

/**
 * Segment class. 
 */
template <typename Value, unsigned dim>
	class Segment : public Object<Value, dim>
{
protected:
	typedef Value value_type ; 
	typedef typename vector <Value, dim>::point_type point_type ;
	typedef point_type vector_type; 
	typedef Segment<Value, dim>            SegmentType; 
	
public:
	Segment () {} ;
	Segment (const point_type &a, const point_type &b) : p0(a) , p1(b) { };
	point_type p0 ; 
	point_type p1 ;

	/**
	 *Return the square lenght of segment
	 */
	value_type square_length ()
	{
		return (p0-p1).square_module();
	}
	/**
	 * Order two vertices in alphabetic order. 
	 */
	void order () 
	{
		if (!(p0 < p1)) {std::swap (p0, p1);}
	}

    /**
	 * Decide is point on the segment. 
	 * @param _p the point to be tested. 
	 */
	bool is_on (const point_type & _p) const
	{
		point_type pa = p0 - _p ; 
		point_type pb = p1 - _p ;
		value_type d = pa * pb;
		if (d <= epsilon && 
			std::abs(d * d - pa.module2() * pb.module2()) <= epsilon2) return true; 
		return false;
	}

	friend bool operator == (const SegmentType & seg1, const SegmentType & seg2)
	{
		if ( ((distance2(seg1.p0 , seg2.p0) <= epsilon2) &&
			 (distance2(seg1.p1 , seg2.p1) <= epsilon2))
			 ||
			 ((distance2(seg1.p0 , seg2.p1) <= epsilon2) &&
			  (distance2(seg1.p1 , seg2.p0) <= epsilon2)) ) return true; 
		return false; 
	}

	friend bool operator != (const SegmentType & seg1, const SegmentType & seg2)
	{
		return (!(seg1 == seg2));
	}

	friend std::ostream & operator << (std::ostream & ostr, const Segment & seg)
	{
		ostr << seg.p0<<' '<<seg.p1 ;
		return ostr; 
	}
};


/**
 * Ray class. 
 */
template <typename value_, unsigned dim>
	class Ray : public Object<value_, dim>
{
protected:
	typedef value_ value_type ; 
	typedef typename vector <value_type, dim>::point_type point_type ;
	typedef vector <value_type, dim>            vector_type ; 
	typedef Segment<value_type, dim>            SegmentType; 

public :
	/**
	 * Default constructor.
	 */
	Ray () : m_point(vector_type (0,0,0)), m_dir (vector_type (0,0,0)){}

	/**
	 * Specify the start point and the direction.
	 * @param _p start point.
	 * @param _n the direction.
	 */
	Ray (const point_type & _p, const vector_type & _n) : m_point (_p), m_dir(_n)
	{
	}
	
	const point_type & get_point () const { return this->m_point; }
	const vector_type & get_direction() const {return this->m_dir ;}
	friend std::ostream & operator << (std::ostream & ostr, const Ray & ray )
	{
		ostr << ray.m_point << ray.m_dir ;
		return ostr; 
	}
private:
	point_type m_point ;
	vector_type m_dir; 
};

/**
 * Polygon class. We assume this polygon is embedded in a plane. 
 */
template <typename Value, unsigned dim>
	class Polygon : public Object<Value, dim>
{
protected:
	typedef Value value_type ; 
	typedef typename vector <Value, dim>::point_type point_type ;
	typedef vector <Value, dim>            vector_type ; 
	typedef Plane <Value, dim>            PlaneType ; 

public :

	/**
	 * Default constructor. 
	 */
	Polygon () {}

	/**
	 * Copy constructor. 
	 * @param _p Polygon object.
	 */
	Polygon (const Polygon & _p) 
		:m_vertices (_p.m_vertices){}

    /**
	 * Construct polygon from a point vector. 
	 */
	Polygon (const std::vector<point_type> & _vp) 
	{
		for (unsigned i = 0; i < _vp.size(); ++i)
		{
			this->m_vertices.push_back(_vp[i]);
		}
	}

	/**
	 * Return the number of vertices in polygon.
	 */
	unsigned size() const 
	{
		return this->m_vertices.size();
	}

    /**
	 * Construct polygon from a point vector. 
	 */
	Polygon (const std::vector<point_type *> & _vp) 
	{
		for (unsigned i = 0; i < _vp.size(); ++i)
		{
			this->m_vertices.push_back(*_vp[i]);
		}
	}

	/**
	 * Remove all the veritices in the polygon. 
	 * The polygon size will be zero.
	 */
	void clear(){this->m_vertices.clear();}

public:
	/**
	 * Decide if the point is inside of the polygon (convex or nonconvex). 
	 - Check if the point and the polygon are co-plane. 
	 - Map the point and the polygon to 2D domain. 
	 - Decide the relation in 2D space. 
	 @code 
    int pnpoly(int npol, value_type *xp, value_type *yp, value_type x, value_type y)
    {
      int i, j, c = 0;
      for (i = 0, j = npol-1; i < npol; j = i++) {
        if ((((yp[i] <= y) && (y < yp[j])) ||
             ((yp[j] <= y) && (y < yp[i]))) &&
            (x < (xp[j] - xp[i]) * (y - yp[i]) / (yp[j] - yp[i]) + xp[i]))
          c = !c;
      }
      return c;
    }
	@endcode
	 */
	bool is_on (const point_type & _p) const 
	{
		unsigned size = m_vertices.size();
		// Check if the point is on the vertex .
		for (unsigned i = 0; i < size; ++ i)
		{
			if ((m_vertices[i] - _p ).square_module() <= epsilon2)
			{
				return true; 
			}
		}
		// Map the polygon and point to the 2D domain. 
		unsigned x, y;
		select_2Dmap_axis (x, y) ;
		
		bool rvalue = false; 
		for (unsigned i = 0, j = size - 1; i < size; j = i ++)
		{
			if ((((m_vertices[i][y] <= _p[y]) && (_p[y] < m_vertices[j][y])) ||
				 ((m_vertices[j][y] <= _p[y]) && (_p[y] < m_vertices[i][y]))) &&
				(_p[x] < (m_vertices[j][x] - m_vertices[i][x]) * (_p[y] - m_vertices[i][y]) / ( m_vertices[j][y] -  m_vertices[i][y]) + m_vertices[i][x]))
				rvalue = !  rvalue;  					
		}
		return rvalue; 
	}

	/**
	 * return the number of vertices.
	 */
	unsigned get_nb_vertices () const
	{
		return m_vertices.size();
	}

    /**
	 * set the number of vertices. 
	 * @param unsigned size; 
	 */
	void set_nb_vertices (unsigned _size) const
	{
		m_vertices.resize(_size);
	}

	/**
	 * return the l-value reference of vertices. 
	 */
	point_type & get_vertex (unsigned idx) 
	{
		return m_vertices[idx];
	}

    /**
	 * return the r-value reference of vertices. 
	 */
	const point_type & operator [](unsigned idx)  const
	{
		return m_vertices[idx];
	}
    /**
	 * return the l-value reference of vertices. 
	 */
	point_type & operator [](unsigned idx)  
	{
		return m_vertices[idx];
	}

	/**
	 * Calculate the normal of polygon.
	 * The vertices are ordered in counter-clockwise. 
	 - This function will not change the normal of this polygon. 
	 */
    vector_type calculate_normal () const 
	{
		if (m_vertices.size() <3) throw ("A face with less than 3 vertices does not have a normal") ;
		return   (m_vertices[1] - m_vertices[0]) ^ (m_vertices[2] - m_vertices[0]) ;
	}


	// Get/Set the normal of the polygon
    /**
     * Returns the normal of this polygon.
     * For performance reason, the normal is precalculated.
     * @return The normal of the polygon.
     */
    const vector_type & get_normal() const 
	{
		return this->m_normal; 
	}

    /**
     * Sets the normal for this polygon.
     * @param normal The normal of the polygon.
     */
    void set_normal(const vector_type & _n) { this->m_normal = _n; }

	/**
	 * Get the plane that the polygon embedded. 
	 * pre {update_normal()}
	 */
	const PlaneType get_plane () const
	{

		return PlaneType (m_vertices[0], this->m_normal );
	}

	/**
	 * Get the area of the polygon. 
	 * Refer to 
	 * @link 
	 http://mathworld.wolfram.com/PolygonArea.html 
	 Area 
	 * @endlink
	 * @return the area of the polygon. It is always non-negative. 
	 * @pre {The function update_normal() is called. }
	 */
	Value get_area () 
	{
		vector_type normal = m_normal ;
		normal.normalize();

		value_type area = value_type(0);
		value_type an = normal.module();
		value_type ax, ay, az ;
		unsigned coord ; // coord to be ignored. 1-x, 2-y, 3-z
		
		ax = std::abs(normal[0]) ;
		ay = std::abs(normal[1]) ;
		az = std::abs(normal[2]) ;
		
		coord = 2; 

		if (ax > ay )
		{
			if (ax > az) coord = 0;
		}
		else
		{
			if (ay > az) coord = 1 ;
		}


		unsigned size = m_vertices.size();
		for (unsigned k = 0; k < size ; ++k )
		{
			unsigned i = (k + 1) % size  ;
			switch (coord)
			{
			case 0:
				area  += (m_vertices[k][1] * m_vertices[i][2] - m_vertices[k][2] * m_vertices[i][1] );
				break; 
			case 1:
				area  += (m_vertices[k][0] * m_vertices[i][2] - m_vertices[k][2] * m_vertices[i][0] );
				break; 
			case 2:
				area  += (m_vertices[k][0] * m_vertices[i][1] - m_vertices[k][1] * m_vertices[i][0] );
				break; 
			}
		}
		switch (coord) {
		case 0:
			area *= (an / (2*ax));
			break;
		case 1:
			area *= (an / (2*ay));
			break;
		case 2:
			area *= (an / (2*az));
		}
		return abs(area) ;
	}

	void append_vertex (const point_type & _v)
	{
		this->m_vertices.push_back(_v);
	}
	friend std::ostream & operator << (std::ostream & ostr, const Polygon & _p)
	{
		for (unsigned i = 0;i  < _p.m_vertices.size(); ++i)
		{
			ostr<< _p.m_vertices [i] <<' ';
		}
		return ostr; 
	}

public:
	/**
	 * Decide along which axis to do 2D map. 
	 * @return 0-x, 1-y, 2-z; 
	 */
	unsigned select_2Dmap_axis(unsigned & x, unsigned & y) const 
	{
		unsigned coord = max_index (m_normal);
		x = (coord + 1) % 3; 
		y = (coord + 2) % 3;		
		return coord; 
	}
protected :
	std::vector <point_type> m_vertices ;
	vector_type m_normal;
};



/**
 * 3D Plane class. 
 * A plane is defined by ax + by + cz + d = 0. 
 */
template <typename Value, unsigned dim>
	class Plane : public Object<Value, dim>
{
protected:
	typedef Value value_type ; 
	typedef typename vector <Value, dim>::point_type point_type ;
	typedef vector <Value, dim>            vector_type ; 

public :

    /**
     * Constructs an uninitialised plane.
     */
    Plane() {}

	/**
	 * Constucts a plane with the point _p and the normal _n
	 * @param _p One point on the plane.
	 * @param _n The positive direction of plane. 
	 */
	Plane (const point_type & _p, const vector_type & _n)
	{
		fromPointAndNormal(_p, _n);
	}

	Plane (const Polygon <value_type, dim> & pol)
	{
		vector_type v0 (pol[0] - pol[1]);
		vector_type v1 (pol[2] - pol[1]);
		vector_type n = v1 ^ v0 ;
		n.normalize();
		a = n.x ;
		b = n.y ;
		c = n.z ;
		d = -(pol[0].x * a + pol[0].y * b + pol[0].z * c);		
	}
    /**
     * Constructs an initialised plane.
     * @param a The a factor.
     * @param b The b factor.
     * @param c The c factor.
     * @param d The d factor.
     */
    Plane(float _a, float _b, float _c, float _d) : a(_a), b(_b), c(_c), d(_d) {}

    /**
     * Constructs a plane from three points.
     * @param p0 1th point.
     * @param p1 2nd point.
     * @param p2 3th point.
     */
    void fromPoints(const point_type &p0, const point_type &p1, const point_type &p2)
	{
		vector_type v0 (p0 - p1);
		vector_type v1 (p2 - p1);
		vector_type n = v1 ^ v0 ;
		n.normalize();
		a = n.x ;
		b = n.y ;
		c = n.z ;
		d = -(p0.x * a + p0.y * b + p0.z * c);
	}

    /**
     * Constructs a plane from three points and don't normalize the equation.
     * @param p0 1th point.
     * @param p1 2nd point.
     * @param p2 3th point.
     */
    void fromPointsNN(const point_type &p0, const point_type &p1, const point_type &p2)
	{
		vector_type v0 (p0 - p1);
		vector_type v1 (p2 - p1);
		vector_type n = v1 ^ v0 ;
		a = n.x ;
		b = n.y ;
		c = n.z ;
		d = -(p0.x * a + p0.y * b + p0.z * c);		
	}

    /**
     *  Constructs a plane from a point and a normal.
     * @param p The point that lies on the plane.
     * @param n The normal of the plane.
     */
    void fromPointAndNormal(const point_type &p0, const vector_type &n)
	{
		vector_type normal = n ;
		normal.normalize();
		a = normal[0] ;
		b = normal[1] ;
		c = normal[2] ;
		d = -(p0[0] * a + p0[1] * b + p0[2] * c); 
	}

    /**
     * Makes a dot product between a point and the plane. Result becomes closer to d's factor as the point is near to the plane.
     * @param p The point.
     * @return The dot product.
     */
    value_type dot(const vector_type &p) const
    {
		return a * p.x + b * p.y + c * p.z;
    }

    /**
     * Calculates the distance between a point and the plane.
     * @param p The point.
     * @return The distance.
     */
    float dist(const vector_type &p) const
    {
		return a * p[0] + b * p[1] + c * p[2] + d;
    }

    /**
     * Reflects a vector on the plane.
     * @param p The vector to be reflected.
     * @return The reflected vector.
     */
    vector_type reflect(const vector_type &vec) const 
    {
        value_type d = dist(vec);
        return  vec + vector_type(-a, -b, -c) * d * 2.0f;
    }

    /**
     * Projects a point onto the plane.
     * @param p The point to be projected.
     * @returned the projected point.
     */
	vector_type project(const vector_type &p) const
    {
        float h = dist(p);
        return vector_type(   p[0] - a * h,
						  p[1] - b * h,
						  p[2] - c * h);
    }

    /**
     * Tells if a point is on the plane or not.
     * @param p The point to test.
     * @param (optional) The threshold to use to determine if the point is on the plane or not. Indeed we couldn't just test if the distance from the point to the plane is zero because of potential numerical error. Default is 10^-3.
     * @return true if the point is on the plane, false otherwhise.
     */
    bool isOnPlane(const vector_type &p, float threshold = epsilon)
    {
        float d = dist(p);
        if (d <= threshold && d >= -threshold)
            return true;
        return false;
    }

    /**
     * Determines if a line and the place intersect.
     * @param p0 The first point of the line.
     * @param p1 The second point of the line.
     * @param t A value that will be filled with the distance to travel to get the intersection.
     * @return If the plane and the line are parallel false is returned, true otherwhise since the line obviously intersects with the plane.
     */
    bool intersectWithLine(const vector_type &p0, const vector_type &p1, value_type &t)
    {
        vector_type dir = p1 - p0;
        value_type div = dot(dir);
        if (div == 0)
            return false;

		t = -dist(p0) / div;
        return true;
    }


	/**
	 * Get parameters. 
	 */
	void get_parameter (value_type & _a, value_type & _b, value_type & _c, value_type & _d) const 
	{
		_a = a; _b = b ; _c = c; _d = d; 
	}
private :
	/**A plane is defined by a x + b y + c z + d = 0*/
	value_type a , b , c , d; 
};

/**
 * Sphere class. 
 */
template <typename value_, unsigned dim>
	class Sphere : public Object<value_, dim>
{
public:
	typedef value_ value_type ; 
	typedef typename vector <value_type, dim>::point_type point_type ;
	typedef vector <value_type, dim>            vector_type ; 
	typedef Sphere <value_type, dim>            sphere_type ; 
	typedef Segment <value_type, dim>            segment_type ; 
public:
	Sphere () {};
	Sphere (const vector_type & _center, const value_type & _radius ) : m_center (_center), m_radius(_radius) {}

	bool is_point_inside (const point_type & point) const
	{
		return distance2 (point, m_center) <= m_radius  * m_radius; 
	}
	bool is_collide_with_segment (const segment_type & seg)
	{
		return false ;
	}
	bool is_collide_with_radius (const sphere_type & s) const 
	{
		return distance2 (s.m_center, this->m_center) <= (s.m_radius + this->m_radius) * (s.m_radius + this->m_radius);
	}
	const vector_type & get_center () const {return this->m_center; }
	const value_type & get_radius () const {return this->m_radius;}
	void set_center (const vector_type & c){this->m_center = c; }
	void set_radius (const value_type & r) {this->m_radius = r; }
private:
	point_type m_center; 
	value_type m_radius ;
};

template <typename Value, unsigned dim>
	class Ellipsoid : public Object<Value, dim>
{
};

enum PLANE
{
	LEFT_PLANE , RIGHT_PLANE, BOTTOM_PLANE, TOP_PLANE, NEAR_PLANE, FAR_PLANE
};



/**
 * Box class. Axias aligned box. 
 */
template <typename Value, unsigned dim>
	class Box : public Object<Value, dim >
{
protected :
	typedef Value value_type ; 
	typedef typename vector <Value, dim>::point_type point_type ;
	typedef vector <Value, dim>                     vector_type ; 
	typedef Plane <Value, dim>                      PlaneType ;
	typedef Polygon<Value, dim>                     PolygonType ;
	/**
	 * We use segment to show the edges of box. 
	 */
	typedef Segment<Value, dim>                     BoxEdgeType ;
public: 
	Box () {}
	Box (const vector_type & _c, const vector_type & _e) : center (_c), extent (_e) {}
	Box (const PolygonType & _p) 
	{
		throw ("Incomplete function. Contruct box from polygon.");
	}
	~Box (){}

	/**
	 * Get the minimum coordinate vertex. 
	 */
	vector_type getMin() const { return center - extent ;}

	/**
	 * Get the maximum coordinate vertex. 
	 */
	vector_type getMax() const { return center + extent ;}

	/**
	 * Get the center of the box. 
	 */
	vector_type getCenter () const {return this->center ;}

	/**
	 * Get the extent of the box. 
	 */
	vector_type getExtent () const {return this->extent ;}

	/**
	 * Return the volumn of the box. 
	 */
	value_type get_volumn () const 
	{
		return abs (value_type(8) * extent[0] * extent[1] * extent[2] ) ;
	}

	void setFromPoints(const vector_type &_pt0, const vector_type &_pt1)
    {
        center = (_pt1 + _pt0) * value_type(0.5);
        extent = (_pt1 - _pt0) * value_type(0.5);
        extent = abs(extent);
    }
	
	bool isOverlap (const Box & _box) const 
	{ 
		return isOverlapX(_box) && isOverlapY (_box) && isOverlapZ(_box) ;
	}
	bool isInside(const Box &_box) const
	{

		for (unsigned i = 0; i < dim ; ++i)
		{
			if (center[i] - extent[i] > _box.center[i] - _box.extent[i] &&
				center[i] + extent[i] < _box.center[i] + _box.extent[i]) ;
			else return false; 
		}
		return true; 

/*
		return (center.x - extent.x > _box.center.x - _box.extent.x &&
			center.x + extent.x < _box.center.x + _box.extent.x &&

			center.y - extent.y > _box.center.y - _box.extent.y &&
			center.y + extent.y < _box.center.y + _box.extent.y && 

			center.z - extent.z > _box.center.z - _box.extent.z &&
			center.z + extent.z < _box.center.z + _box.extent.z) ;
*/
	}
	bool isInside (const vector_type & _pt) const 
	{
		vector_type min = getMin();
		vector_type max = getMax ();

		for (unsigned i = 0; i < dim ; ++i)
		{
			if  (_pt[i] >= min[i] && _pt[i] <= max[i] ) ; else return false; 
		}
		return true; 
/*
		
		return (_pt[0] >= min[0] && _pt[0] <= max[0] 
			&& _pt[1] >= min[1] && _pt[1] <= max[1]
			&& _pt[2] >= min[2] && _pt[2] <= max[2]) ;
*/		
	}
	vector_type getVertex (int _index) const 
	{
		switch(_index) 
		{
		case 0: return vector_type(center.x - extent.x, center.y - extent.y, center.z - extent.z);
		case 1: return vector_type(center.x + extent.x, center.y - extent.y, center.z - extent.z);
		case 2: return vector_type(center.x - extent.x, center.y + extent.y, center.z - extent.z);
		case 3: return vector_type(center.x + extent.x, center.y + extent.y, center.z - extent.z);
		case 4: return vector_type(center.x - extent.x, center.y - extent.y, center.z + extent.z);
		case 5: return vector_type(center.x + extent.x, center.y - extent.y, center.z + extent.z);
		case 6: return vector_type(center.x - extent.x, center.y + extent.y, center.z + extent.z);
		case 7: return vector_type(center.x + extent.x, center.y + extent.y, center.z + extent.z);
		default:
			return vector_type ( 0.0f, 0.0f, 0.0f);
		}
	}

	static vector_type getFaceNormal (PLANE _plane)
	{
		switch(_plane)
		{
		case LEFT_PLANE : return vector_type( -1.0f, 0.0f, 0.0f);
		case RIGHT_PLANE: return vector_type( 1.0f, 0.0f, 0.0f);
		case BOTTOM_PLANE: return vector_type( 0.0f, -1.0f, 0.0f);
		case TOP_PLANE:return vector_type( 0.0f, 1.0f, 0.0f);
		case NEAR_PLANE:return vector_type( 0.0f, 0.0f, -1.0f);
		case FAR_PLANE:return vector_type( 0.0f, 0.0f, 1.0f);
		default:
			return vector_type(0.0f, 0.0f, 0.0f); 
		}
	}

	PolygonType getFacePolygon (int _idx) const
	{
		switch (_idx)
		{
		case 0 : break;
		case 1 : break;
		case 2 : break;
		case 3 : break;
		case 4 : break;
		case 5 : break;
		case 6 : break;
		}
	}
	PlaneType getPlane(PLANE _which) const
    {
        PlaneType plane;
        switch(_which) {
            case 0: plane.fromPointAndNormal(vector_type(center.x - extent.x, 0, 0), getFaceNormal(_which));
            case 1: plane.fromPointAndNormal(vector_type(center.x + extent.x, 0, 0), getFaceNormal(_which));
            case 2: plane.fromPointAndNormal(vector_type(0, center.y - extent.y, 0), getFaceNormal(_which));
            case 3: plane.fromPointAndNormal(vector_type(0, center.y + extent.y, 0), getFaceNormal(_which));
            case 4: plane.fromPointAndNormal(vector_type(0, 0, center.z - extent.z), getFaceNormal(_which));
            case 5: plane.fromPointAndNormal(vector_type(0, 0, center.z + extent.z), getFaceNormal(_which));
            default: plane = PlaneType(0, 0, 0, 0);
        }
        return plane;
    }

    BoxEdgeType getEdge(int _index) const
    {
        value_type x0 = center[0] - extent[0];
        value_type y0 = center[1] - extent[1];
        value_type z0 = center[2] - extent[2];
        value_type x1 = center[0] + extent[0];
        value_type y1 = center[1] + extent[1];
        value_type z1 = center[2] + extent[2];

        switch(_index) {
            case 0: return BoxEdgeType(vector_type(x0, y0, z0), vector_type(x1, y0, z0));
            case 1: return BoxEdgeType(vector_type(x1, y0, z0), vector_type(x1, y1, z0));
            case 2: return BoxEdgeType(vector_type(x1, y1, z0), vector_type(x0, y1, z0));
            case 3: return BoxEdgeType(vector_type(x0, y1, z0), vector_type(x0, y0, z0));

            case 4: return BoxEdgeType(vector_type(x0, y0, z1), vector_type(x1, y0, z1));
            case 5: return BoxEdgeType(vector_type(x1, y0, z1), vector_type(x1, y1, z1));
            case 6: return BoxEdgeType(vector_type(x1, y1, z1), vector_type(x0, y1, z1));
            case 7: return BoxEdgeType(vector_type(x0, y1, z1), vector_type(x0, y0, z1));

            case 8: return BoxEdgeType(vector_type(x0, y0, z0), vector_type(x0, y0, z1));
            case 9: return BoxEdgeType(vector_type(x1, y0, z0), vector_type(x1, y0, z1));
            case 10: return BoxEdgeType(vector_type(x1, y1, z0), vector_type(x1, y1, z1));
		    case 11: return BoxEdgeType(vector_type(x0, y1, z0), vector_type(x0, y1, z1));

            default: return BoxEdgeType(vector_type(0.0f, 0.0f, 0.0f), vector_type(0.0f, 0.0f, 0.0f));
        }
    }

protected:
	bool isOverlapX (const Box & _box) const
	{
		value_type t = _box.center[0] - center[0] ;
		t = t <0 ? (-t) : t ;
		value_type ext = _box.extent[0] + extent[0] ;
		if (t <= ext) return true; 
		return false; 
	}
	bool isOverlapY (const Box & _box) const
	{
		value_type t = _box.center[1] - center[1] ;
		t = t <0 ? (-t) : t ;
		value_type ext = _box.extent[1] + extent[1] ;
		if (t <= ext) return true; 
		return false; 
	
	}
	bool isOverlapZ (const Box & _box) const 
	{
		value_type t = _box.center[2] - center[2] ;
		t = t <0 ? (-t) : t ;
		value_type ext = _box.extent[2] + extent[2] ;
		if (t <= ext) return true; 
		return false; 
	
	}
public :
	vector_type center ;
	vector_type extent ;
};


/**
 * Defines an OBB (Oriented Bounding Box) which is merely an oriented AABB.
 */
template <typename value_, unsigned dim>
class OBB : public Box <value_, dim>
{
protected:
	typedef matrix<value_, 3, 3> matrix_type ;
public:
	
    matrix_type m_matrix; /**< The rotation matrix that affects the Box to obtain that OBB.*/
};


enum POSITION
{
	NONINTERSECT, OVERLAP, INTERSECT
};

enum PRIMITIVE_TYPE
{
	POINT, SEGMENT, LINE, RAY, POLYGON, PLANE
}; 

template <typename Value, unsigned dim>
struct IntersectResult
{
	POSITION pos ;
	PRIMITIVE_TYPE res_type ;


	vector<Value, dim>   point ;
	Segment <Value, dim> segment ;
	Line <Value, dim>    line ;
	Polygon <Value, dim> polygon ;
	Plane <Value, dim>   plane ;

	friend std::ostream & operator << (std::ostream & ostr, const IntersectResult & res )
	{
		switch (res.pos)
		{
		case OVERLAP: std::cout<<"overlap"<<std::endl; break;
		case NONINTERSECT : std::cout<<"NONintersect"<<std::endl; break;
		case INTERSECT : std::cout<<"Intersect"<<std::endl; break;;
		}

		if (res.pos == INTERSECT || res.pos == OVERLAP)
		{
			switch (res.res_type)
			{
			case POINT: std::cout<<"Point : " << res.point << std::endl; break; 
			case SEGMENT: std::cout<<"Segment : " << res.segment<< std::endl; break; 
			case LINE: std::cout<<"Line : " << res.line<< std::endl; break; 
			case POLYGON: std::cout<<"Polygon : " << res.polygon<< std::endl; break; 
			}
		}
		return ostr ;
	}
};

}

#endif
